
# include "lqr_vmc_controller/fivethplanner.hpp"

# include <cmath>
# include <stdlib.h>
# include <iostream>

namespace lqr_vmc_controller
{
fivethplanner::fivethplanner()
{
}

void fivethplanner::getfivethParam(double firstpos, double firstvel, double firstacc, double finalpos, double finalvel, double finalacc, double timehorizon, double update_rate)
{
    timehorizon_ = timehorizon;
    update_rate_ = update_rate;
    double h = finalpos - firstpos;
    fivethparam_[0] = firstpos;
    fivethparam_[1] = firstvel;
    fivethparam_[2] = firstacc/2.0;
    fivethparam_[3] = (20*h - (8*finalvel + 12*firstvel)*timehorizon - (3*firstacc - finalacc)*(pow(timehorizon,2)))/(2*pow(timehorizon,3));
    fivethparam_[4] = (-30*h + (14*finalvel + 16*firstvel)*timehorizon + (3*firstacc - 2*finalacc)*(pow(timehorizon,2)))/(2*pow(timehorizon,4));
    fivethparam_[5] = (12*h - (6*finalvel + 6*firstvel)*timehorizon + (finalacc -firstacc)*(pow(timehorizon,2)))/(2*pow(timehorizon,5));
}

void fivethplanner::startplanner()
{
    time_ =  0.0;
    planningflag_ = true;
    return ;
}

double fivethplanner::getPosition() // 规划中才可以调用
{
    if (time_ <= timehorizon_)
    {
        double result = (fivethparam_[0] + fivethparam_[1]*time_ + fivethparam_[2]*pow(time_, 2) + fivethparam_[3]*pow(time_, 3) + fivethparam_[4]*pow(time_, 4) + fivethparam_[5]*pow(time_, 5));
        time_ += (1.0/update_rate_);
        return result;
    }
    else 
    {
        planningflag_ = false;
        return (fivethparam_[0] + fivethparam_[1]*timehorizon_ + fivethparam_[2]*pow(timehorizon_, 2) + fivethparam_[3]*pow(timehorizon_, 3) + fivethparam_[4]*pow(timehorizon_, 4) + fivethparam_[5]*pow(timehorizon_, 5));
    }
}

bool fivethplanner::isplanning()
{
    return planningflag_;
}
}